
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_imu_bmi055.c
  * @author     baiyang
  * @date       2022-6-5
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_imu_bmi055.h"

#include <common/time/gp_time.h>
#include <common/console/console.h>
/*-----------------------------------macro------------------------------------*/
/*
  device registers, names follow datasheet conventions, with REGA_
  prefix for accel, and REGG_ prefix for gyro
 */
#define REGA_BGW_CHIPID    0x00
#define REGA_ACCD_X_LSB    0x02
#define REGA_ACCD_TEMP     0x08
#define REGA_INT_STATUS_0  0x09
#define REGA_INT_STATUS_1  0x0A
#define REGA_INT_STATUS_2  0x0B
#define REGA_INT_STATUS_3  0x0C
#define REGA_FIFO_STATUS   0x0E
#define REGA_PMU_RANGE     0x0F
#define REGA_PMU_BW        0x10
#define REGA_PMU_LPW       0x11
#define REGA_ACCD_HBW      0x13
#define REGA_BGW_SOFTRESET 0x14
#define REGA_OUT_CTRL      0x20
#define REGA_EST_LATCH     0x21
#define REGA_FIFO_CONFIG_0 0x30
#define REGA_PMU_SELF_TEST 0x32
#define REGA_FIFO_CONFIG_1 0x3E
#define REGA_FIFO_DATA     0x3F

#define REGG_CHIPID        0x00
#define REGA_RATE_X_LSB    0x02
#define REGG_INT_STATUS_0  0x09
#define REGG_INT_STATUS_1  0x0A
#define REGG_INT_STATUS_2  0x0B
#define REGG_INT_STATUS_3  0x0C
#define REGG_FIFO_STATUS   0x0E
#define REGG_RANGE         0x0F
#define REGG_BW            0x10
#define REGG_LPM1          0x11
#define REGG_RATE_HBW      0x13
#define REGG_BGW_SOFTRESET 0x14
#define REGG_FIFO_CONFIG_1 0x3E
#define REGG_FIFO_DATA     0x3F

#define ACCEL_BACKEND_SAMPLE_RATE   2000
#define GYRO_BACKEND_SAMPLE_RATE    2000

#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void sensor_imu_bmi055_start(sensor_imu_backend *backend);
static bool sensor_imu_bmi055_update(sensor_imu_backend *backend);
static bool accel_init(sensor_imu_bmi055_t bmi055);
static bool gyro_init(sensor_imu_bmi055_t bmi055);
static bool init(sensor_imu_bmi055_t bmi055);
static void read_fifo_accel(void *parameter);
static void read_fifo_gyro(void *parameter);
/*----------------------------------variable----------------------------------*/
static struct sensor_imu_backend_ops bmi055_ops;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_imu_bmi055_ctor(sensor_imu_bmi055_t bmi055,
                                    gp_device_t _dev_accel,
                                    gp_device_t _dev_gyro,
                                    enum RotationEnum _rotation)
{
    // 清空sensor_imu_backend结构体变量，因为sensor_imu_backend结构体有可能是申请的动态内存
    // 防止sensor_imu_backend中的变量初始为非零值。
    rt_memset(bmi055, 0, sizeof(struct sensor_imu_bmi055));

    sensor_imu_backend_ctor(&bmi055->backend, "bmi055");

    bmi055_ops = imu_backend_ops;

    bmi055_ops.start = sensor_imu_bmi055_start;
    bmi055_ops.update = sensor_imu_bmi055_update;

    bmi055->backend.ops = &bmi055_ops;

    bmi055->rotation = _rotation;
    
    bmi055->dev_accel = _dev_accel;
    bmi055->dev_gyro = _dev_gyro;
}

sensor_imu_backend * sensor_imu_bmi055_probe(gp_device_t dev_accel,
                                gp_device_t dev_gyro,
                                enum RotationEnum rotation)
{
    if (!dev_accel || !dev_gyro) {
        return NULL;
    }

    sensor_imu_bmi055_t sensor = (sensor_imu_bmi055_t)rt_malloc(sizeof(struct sensor_imu_bmi055));

    if (!sensor) {
        return NULL;
    }

    sensor_imu_bmi055_ctor(sensor, dev_accel, dev_gyro, rotation);

    if (!init(sensor)) {
        sensor_imu_backend_destructor(&sensor->backend);
        rt_free(sensor);
        return NULL;
    }

    return (sensor_imu_backend *)sensor;
}

static void sensor_imu_bmi055_start(sensor_imu_backend *backend)
{
    sensor_imu_bmi055_t bmi055 = (sensor_imu_bmi055_t)backend;

    if (!sensor_imu_register_accel(&bmi055->accel_instance, ACCEL_BACKEND_SAMPLE_RATE, devmgr_get_bus_id_devtype(bmi055->dev_accel, DEVTYPE_INS_BMI055)) ||
        !sensor_imu_register_gyro(&bmi055->gyro_instance, GYRO_BACKEND_SAMPLE_RATE, devmgr_get_bus_id_devtype(bmi055->dev_gyro, DEVTYPE_INS_BMI055))) {
        return;
    }

    // setup sensor rotations from probe()
    sensor_imu_backend_set_gyro_orientation(bmi055->gyro_instance, bmi055->rotation);
    sensor_imu_backend_set_accel_orientation(bmi055->accel_instance, bmi055->rotation);
    
    // setup callbacks
    devmgr_register_periodic_callback(bmi055->dev_accel, 1000000UL / ACCEL_BACKEND_SAMPLE_RATE, read_fifo_accel, bmi055);
    devmgr_register_periodic_callback(bmi055->dev_gyro, 1000000UL / GYRO_BACKEND_SAMPLE_RATE, read_fifo_gyro, bmi055);
}

static bool sensor_imu_bmi055_update(sensor_imu_backend *backend)
{
    sensor_imu_bmi055_t bmi055 = (sensor_imu_bmi055_t)backend;

    sensor_imu_backend_update_accel(backend, bmi055->accel_instance);
    sensor_imu_backend_update_gyro(backend, bmi055->gyro_instance);
    return true;
}

static bool accel_init(sensor_imu_bmi055_t bmi055)
{
    devmgr_take_bus(bmi055->dev_accel);

    uint8_t v;
    if (!devmgr_read_registers(bmi055->dev_accel, REGA_BGW_CHIPID, &v, 1) || v != 0xFA) {
        goto failed;
    }

    if (!devmgr_write_register(bmi055->dev_accel, REGA_BGW_SOFTRESET, 0xB6, false)) {
        goto failed;
    }
    rt_thread_mdelay(10);
    
    devmgr_setup_checked_registers(bmi055->dev_accel, 5, 20);
    
    // setup 16g range
    if (!devmgr_write_register(bmi055->dev_accel, REGA_PMU_RANGE, 0x0C, true)) {
        goto failed;
    }

    // setup filter bandwidth 1kHz
    if (!devmgr_write_register(bmi055->dev_accel, REGA_PMU_BW, 0x0F, true)) {
        goto failed;
    }

    // disable low-power mode
    if (!devmgr_write_register(bmi055->dev_accel, REGA_PMU_LPW, 0, true)) {
        goto failed;
    }

    // setup for unfiltered data
    if (!devmgr_write_register(bmi055->dev_accel, REGA_ACCD_HBW, 0x80, true)) {
        goto failed;
    }

    // setup FIFO for streaming X,Y,Z
    if (!devmgr_write_register(bmi055->dev_accel, REGA_FIFO_CONFIG_1, 0x80, true)) {
        goto failed;
    }

    console_printf("BMI055: found accel\n");

    devmgr_release_bus(bmi055->dev_accel);
    return true;
    
failed:
    devmgr_release_bus(bmi055->dev_accel);
    return false;
}

/*
  probe and initialise gyro
 */
static bool gyro_init(sensor_imu_bmi055_t bmi055)
{
    devmgr_take_bus(bmi055->dev_gyro);

    uint8_t v;
    if (!devmgr_read_registers(bmi055->dev_gyro, REGG_CHIPID, &v, 1) || v != 0x0F) {
        goto failed;
    }

    if (!devmgr_write_register(bmi055->dev_gyro, REGG_BGW_SOFTRESET, 0xB6, false)) {
        goto failed;
    }
    rt_thread_mdelay(10);

    devmgr_setup_checked_registers(bmi055->dev_gyro, 5, 20);
    
    // setup 2000dps range
    if (!devmgr_write_register(bmi055->dev_gyro, REGG_RANGE, 0x00, true)) {
        goto failed;
    }

    // setup filter bandwidth 230Hz, no decimation
    if (!devmgr_write_register(bmi055->dev_gyro, REGG_BW, 0x81, true)) {
        goto failed;
    }

    // disable low-power mode
    if (!devmgr_write_register(bmi055->dev_gyro, REGG_LPM1, 0, true)) {
        goto failed;
    }

    // setup for filtered data
    if (!devmgr_write_register(bmi055->dev_gyro, REGG_RATE_HBW, 0x00, true)) {
        goto failed;
    }

    // setup FIFO for streaming X,Y,Z
    if (!devmgr_write_register(bmi055->dev_gyro, REGG_FIFO_CONFIG_1, 0x80, true)) {
        goto failed;
    }

    console_printf("BMI055: found gyro\n");

    devmgr_release_bus(bmi055->dev_gyro);
    return true;
    
failed:
    devmgr_release_bus(bmi055->dev_gyro);
    return false;
}

static bool init(sensor_imu_bmi055_t bmi055)
{
    devmgr_set_read_flag(bmi055->dev_accel, 0x80);
    devmgr_set_read_flag(bmi055->dev_gyro, 0x80);

    return accel_init(bmi055) && gyro_init(bmi055);
}

/*
  read accel fifo
 */
static void read_fifo_accel(void *parameter)
{
    sensor_imu_bmi055_t bmi055 = (sensor_imu_bmi055_t)parameter;

    uint8_t num_frames;
    if (!devmgr_read_registers(bmi055->dev_accel, REGA_FIFO_STATUS, &num_frames, 1)) {
        sensor_imu_backend_inc_accel_error_count(bmi055->accel_instance);
        return;
    }
    num_frames &= 0x7F;
    
    // don't read more than 8 frames at a time
    if (num_frames > 8) {
        num_frames = 8;
    }

    if (num_frames == 0) {
        return;
    }
    
    uint8_t data[6*num_frames];
    if (!devmgr_read_registers(bmi055->dev_accel, REGA_FIFO_DATA, data, num_frames*6)) {
        sensor_imu_backend_inc_accel_error_count(bmi055->accel_instance);
        return;
    }
    // data is 12 bits with 16g range, 7.81mg/LSB
    const float scale = 7.81 * 0.001 * GRAVITY_MSS / 16.0f;
    for (uint8_t i = 0; i < num_frames; i++) {
        const uint8_t *d = &data[i*6];
        int16_t xyz[3] = {
                (int16_t)((uint16_t)((d[0]&0xF0) | (d[1]<<8))),
                (int16_t)((uint16_t)((d[2]&0xF0) | (d[3]<<8))),
                (int16_t)((uint16_t)((d[4]&0xF0) | (d[5]<<8))) };
        Vector3f_t accel = {xyz[0], xyz[1], xyz[2]};

        vec3_mult(&accel, &accel, scale);

        sensor_imu_backend_rotate_and_correct_accel(&bmi055->backend, bmi055->accel_instance, &accel);
        sensor_imu_backend_notify_new_accel_raw_sample(&bmi055->backend, bmi055->accel_instance, &accel, 0, false);
    }

    if (bmi055->temperature_counter++ == 100) {
        bmi055->temperature_counter = 0;
        int8_t t;
        if (!devmgr_read_registers(bmi055->dev_accel, REGA_ACCD_TEMP, (uint8_t *)&t, 1)) {
            sensor_imu_backend_inc_accel_error_count(bmi055->accel_instance);
        } else {
            float temp_degc = (0.5f * t) + 23.0f;
            sensor_imu_backend_publish_temperature(&bmi055->backend, bmi055->accel_instance, temp_degc);
        }
    }

    struct checkreg reg;
    if (!devmgr_check_next_register2(bmi055->dev_accel, &reg)) {
        //log_register_change(dev_accel->get_bus_id(), reg);
        sensor_imu_backend_inc_accel_error_count(bmi055->accel_instance);
    }
}

/*
  read gyro fifo
 */
static void read_fifo_gyro(void *parameter)
{
    sensor_imu_bmi055_t bmi055 = (sensor_imu_bmi055_t)parameter;

    uint8_t num_frames;
    if (!devmgr_read_registers(bmi055->dev_gyro, REGG_FIFO_STATUS, &num_frames, 1)) {
        sensor_imu_backend_inc_gyro_error_count(bmi055->gyro_instance);
        return;
    }
    num_frames &= 0x7F;
    
    // don't read more than 8 frames at a time
    if (num_frames > 8) {
        num_frames = 8;
    }
    if (num_frames == 0) {
        return;
    }
    uint8_t data[6*num_frames];
    if (!devmgr_read_registers(bmi055->dev_gyro, REGG_FIFO_DATA, data, num_frames*6)) {
        sensor_imu_backend_inc_gyro_error_count(bmi055->gyro_instance);
        return;
    }

    // data is 16 bits with 2000dps range
    const float scale = radians(2000.0f) / 32767.0f;
    for (uint8_t i = 0; i < num_frames; i++) {
        const uint8_t *d = &data[i*6];
        int16_t xyz[3] = {
                    (int16_t)((uint16_t)(d[0] | d[1]<<8)),
                    (int16_t)((uint16_t)(d[2] | d[3]<<8)),
                    (int16_t)((uint16_t)(d[4] | d[5]<<8)) };
        Vector3f_t gyro = {xyz[0], xyz[1], xyz[2]};

        vec3_mult(&gyro, &gyro, scale);

        sensor_imu_backend_rotate_and_correct_gyro(&bmi055->backend, bmi055->gyro_instance, &gyro);
        sensor_imu_backend_notify_new_gyro_raw_sample(&bmi055->backend, bmi055->gyro_instance, &gyro, 0);
    }

    struct checkreg reg;
    if (!devmgr_check_next_register2(bmi055->dev_gyro, &reg)) {
        //log_register_change(dev_gyro->get_bus_id(), reg);
        sensor_imu_backend_inc_accel_error_count(bmi055->gyro_instance);
    }
}

/*------------------------------------test------------------------------------*/


